﻿
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace RobotsSample
{
    /// <summary>
    /// 仿真蛇构型
    /// </summary>
    public class Sample_SnakeRobot : RobotSampleAbstract
    {
        public override void Run()
        {
            base.Run();
            /*
           *######################################################################
           *####################### 机器人模块ID配置  ###########################
           *######################################################################
           */

            //输入操作臂ID次序
            Console.WriteLine("The id sequence above may be incorrect");
            Console.WriteLine("Please enter the snake robot ids in sequence from down to top （T T T T T T...）。Zero(0) input means to complete");
            ModulesIds.Clear();

            while (true)
            {
                Console.WriteLine($"Please enter the {ModulesIds.Count + 1} T module's Id");
                int mId = 0;
                var mIdStr = Console.ReadLine();
                var b = int.TryParse(mIdStr, out mId);
                if (mId == 0)
                {
                    break;
                }

                ModulesIds.Add(mId);
            }

            Console.WriteLine("Ids  completed");


            /*
             *######################################################################
             *####################### 机器人算法绑定  ###########################
             *######################################################################
             */
            var ids = new byte[ModulesIds.Count];
            for (var i = 0; i < ids.Length; i++)
            {
                ids[i] = (byte)ModulesIds[i];
            }
            Console.WriteLine("Bind to creat a snake robot...");
            RobotId = MiniBotAlgorithmLib.AddRobot((int)RobotTypeEnum.Snake, ids, ids.Length);

            var rs = MiniBotAlgorithmLib.BindRobotModuleApis(RobotId, SetPositionCallback, SetPositionWithSpeedCallback, SetMultiPositionCallback,
                    SetMultiPositionWithSpeedCallback, SetPositionSpeedCallback, SetRunningSpeedCallback, SetTorqueEnableCallback, SetModeCallback);
            Console.WriteLine("Any Key to to run the snake.....");


            /*
	         *######################################################################
	         *####################### 机器人操作控制开始  ###########################
	         *######################################################################
	         */


            //将关节模式转换成位置模式
            for (var i = 0; i < ModulesIds.Count; i++)
            {
                MiniBotControlLib.SetMotorMode((byte)ModulesIds[i], 0);
            }

            Console.WriteLine("Enter any key to run the snake robot ....");
            Console.ReadLine();
            Task.Run(()=> {
                SendCmd(0, 0);
            });


            while (true)
            {
                //操作命令
                var cmd = 1;

                Console.WriteLine($"Commands：");
                Console.WriteLine("1.Go Forward");
                Console.WriteLine("2.Go Back");
                Console.WriteLine("3.Stop");
                Console.WriteLine("4.Turn Left");
                Console.WriteLine("5.Turn Right");
                Console.WriteLine("6.Set Velocity");
                Console.WriteLine("7.Set Swing Range");
                Console.WriteLine("8.Set Swing Frequency");

                var cmdStr = Console.ReadLine();
                int.TryParse(cmdStr, out cmd);

                switch (cmd)
                {

                    case 6:
                        {
                            Console.WriteLine($"Commands：{Environment.NewLine}Enter Velocity Percent(0-100)");
                            int percent = 0;
                            var mIdStr = Console.ReadLine();
                            int.TryParse(mIdStr, out percent);
                            SendCmd(cmd, percent);
                        }
                        break;
                    case 7:
                        {
                            Console.WriteLine($"Commands：{Environment.NewLine}Enter Swing Range Percent(0-100)");
                            int percent = 0;
                            var mIdStr = Console.ReadLine();
                            int.TryParse(mIdStr, out percent);
                            SendCmd(cmd, percent);
                        }
                        break;
                    case 8:
                        {
                            Console.WriteLine($"Commands：{Environment.NewLine}Enter Swing Frequency Percent(0-100)");
                            int percent = 0;
                            var mIdStr = Console.ReadLine();
                            int.TryParse(mIdStr, out percent);
                            SendCmd(cmd, percent);
                        }
                        break;
                    default:
                        {
                            SendCmd(cmd, 0);
                        }
                        break;
                }

                Console.WriteLine("Enter any key to continue ....");

            }

        }

    }
}
